﻿#include "RobotServerPC.h"
#include <QBuffer>
#include <QDebug>
#include <QSharedMemory>
#include <eigen3/Eigen/Dense>
#include "JsonUtil.h"
using namespace Eigen;

static Matrix<float, 3, 3> ROT1, ROT2, ROT3;
//----------------------------------------------------
// realsense安装位置平移
static Vector3f D0(0.0489f, 0, 0);
static Vector3f D(0.0407f, 0.0250f, 0.2719f);
//----------------------------------------------------
// 三台realsense配置
static Matrix<float, 3, 3> ROTs[3] = {ROT1, ROT2, ROT3};
static Matrix<float, 3, 3> TRAN;

RobotServerPC::RobotServerPC(quint16 usPort,
                             int argc,
                             char** argv,
                             QObject* parent)
    : argc_(argc),
      argv_(argv),
      m_bRunRealsense(false),
      RobotServer(usPort, parent)
{
    m_ros_spin_thread =
        std::thread{std::bind(&RobotServerPC::rosSpinThread, this)};

    m_smBaSend = new QSharedMemory("sm_basend", this);
    // m_smImg->create(921600 * 3);  // rgb*3

    m_smSLAM = new QSharedMemory("sm_slam");
    bool b = m_smSLAM->create(16 * 1024 * 1024);
    qDebug() << "create m_smSLAM: " << b;

    m_threadProcessPc =
        std::thread{std::bind(&RobotServerPC::processPc_proc, this)};
}

RobotServerPC::~RobotServerPC()
{
    qDebug() << __FUNCTION__ << " called";
    rclcpp::shutdown();
}

void RobotServerPC::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

int RobotServerPC::parse(unsigned char* buff, qint64 len)
{
    if (!m_smBaSend->isAttached()) {
        m_smBaSend->attach();
        qDebug() << "attach sm_basend";
    }

    quint8* m_buf = const_cast<quint8*>(buff);
    for (int i = 0; i < 4; ++i) {
        qDebug() << "data[" << i << "]: " << quint8(m_buf[i]);
    }

    qDebug() << "len: " << len;

    int nStartIndex = 0;
    while (nStartIndex + 8 < len) {
        if (m_buf[nStartIndex] == 0xfe && m_buf[nStartIndex + 1] == 0xfe &&
            m_buf[nStartIndex + 2] == 0xfe && m_buf[nStartIndex + 3] == 0xfe) {

            qDebug() << "m_buf[nStartIndex+7]: " << m_buf[nStartIndex + 7];
            qDebug() << "m_buf[nStartIndex+6]: " << m_buf[nStartIndex + 6];
            qDebug() << "m_buf[nStartIndex+5]: " << m_buf[nStartIndex + 5];
            qDebug() << "m_buf[nStartIndex+4]: " << m_buf[nStartIndex + 4];

            int nJsonLen = int(m_buf[nStartIndex + 4]) << 24;
            nJsonLen += int(m_buf[nStartIndex + 5]) << 16;
            nJsonLen += int(m_buf[nStartIndex + 6]) << 8;
            nJsonLen += int(m_buf[nStartIndex + 7]);

            qDebug() << "parse jsonLen: " << nJsonLen;

            if (nStartIndex + 8 + nJsonLen > len) {
                return 0;
            }

            QByteArray ba;
            for (int i = 0; i < nJsonLen; ++i) {
                ba.append(m_buf[nStartIndex + 8 + i]);
            }
            qDebug() << "parse ba: " << ba;
            QVariantMap mapJson;
            if (!JsonUtil::byteArray2VarMap(ba, mapJson)) {
                return nStartIndex + 4;
            } else {
                processJson(mapJson);
                return nStartIndex + 8 + nJsonLen;
            }
        } else {
            nStartIndex++;
        }
    }
    return nStartIndex;
}

void RobotServerPC::processJson(const QVariantMap& json)
{
    QString strAction = json.value("action").toString();
    QString strSerial = json.value("serial").toString();
    int nSend = json.value("send", "-1").toInt();

    qDebug() << "RobotServer strSerial: " << strSerial;

    qDebug() << "recv action: " << strAction;
    qDebug() << "recv json: " << json;
    qDebug() << "\n";

    if ("sendrs" == strAction) {
        QVariantMap mapJson;
        mapJson.insert("action", "sendrs");
        mapJson.insert("serial", strSerial);
        mapJson.insert("result", "0000");
        mapJson.insert("errmsg", "ok");
        QByteArray ba = JsonUtil::packData(mapJson, QByteArray());

        // change0425
        qint64 nWrite = 0;
        while (nullptr != m_socketCur && m_socketCur->isValid() &&
               nWrite < ba.size()) {
            nWrite += m_socketCur->write(ba.mid(nWrite));
            qDebug() << "sendrs total: " << ba.size() << "  ,write: " << nWrite;
        }

        m_bRunRealsense = nSend > 0;
        m_robotCmdNode->rsswitch(m_bRunRealsense);
    } else if ("sendlas" == strAction) {
        //        emit sigGetLasData(strSerial);
        QVariantMap mapJson;
        mapJson.insert("action", "sendlas");
        mapJson.insert("serial", strSerial);
        mapJson.insert("result", "0000");
        mapJson.insert("errmsg", "ok");
        QByteArray ba = JsonUtil::packData(mapJson, QByteArray());

        qint64 nWrite = 0;
        while (nullptr != m_socketCur && m_socketCur->isValid() &&
               nWrite < ba.size()) {
            nWrite += m_socketCur->write(ba.mid(nWrite));
            qDebug() << "sendrs total: " << ba.size() << "  ,write: " << nWrite;
        }
        if (1 == nSend) {
            // WebSocket::instance()->startLaser(strSerial);
            m_robotCmdNode->sendCmd("startLaser", strSerial.toStdString());
        } else if (0 == nSend) {
            // WebSocket::instance()->getCloud();
            m_robotCmdNode->sendCmd("getCloud", "");
        } else if (2 == nSend) {
            // WebSocket::instance()->stopLaser();
            m_robotCmdNode->sendCmd("stopLaser", "");
        }
    } else if ("getrspos" == strAction) {
        qDebug() << "getrsPos";
        qDebug() << json;
    } else if ("sendrspos" == strAction) {
        
        // response immediately
        QVariantMap mapJson;
        mapJson.insert("action", "sendrspos");
        mapJson.insert("serial", strSerial);
        mapJson.insert("rsSeq", 1);
        mapJson.insert("result", "0000");
        QByteArray ba = JsonUtil::packData(mapJson, QByteArray());
        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            try {
                // change0425
                qint64 nWrite = m_socketCur->write(ba);
                qDebug() << "try write total: " << ba.size()
                         << "  write:" << nWrite;
            } catch (...) {
            }
        }

        QVariantMap mapLeft = json.value("left").toMap();
        QVariantMap mapRight = json.value("right").toMap();
        // setSendRsPosData(mapLeft, mapRight, strSerial);

        float xl = mapLeft.value("x").toFloat();
        float yl = mapLeft.value("y").toFloat();
        float zl = mapLeft.value("z").toFloat();
        float ll = mapLeft.value("l").toFloat();
        float wl = mapLeft.value("w").toFloat();
        float hl = mapLeft.value("h").toFloat();
        float xr = mapRight.value("x").toFloat();
        float yr = mapRight.value("y").toFloat();
        float zr = mapRight.value("z").toFloat();
        float lr = mapRight.value("l").toFloat();
        float wr = mapRight.value("w").toFloat();
        float hr = mapRight.value("h").toFloat();

        m_robotCmdNode->setSendRsPos(xl, yl, zl, ll, wl, hl, xr, yr, zr, lr, wr,
                                     hr, strSerial.toStdString());
    }
}

// void RobotServerPC::call_tirgger(int leftT, int rightT)
// {
//     QMetaObject::invokeMethod(this, "tirgger", Qt::QueuedConnection,
//                               Q_ARG(int, leftT), Q_ARG(int, rightT));
// }

// void RobotServerPC::call_setGenRParam(int missionNum, const QString& serial)
// {
//     QMetaObject::invokeMethod(this, "setGenRParam", Qt::QueuedConnection,
//                               Q_ARG(int, missionNum), Q_ARG(QString, serial));
// }

// void RobotServerPC::call_opreate(int execStep, const QString& serial)
// {
//     QMetaObject::invokeMethod(this, "opreate", Qt::QueuedConnection,
//                               Q_ARG(int, execStep), Q_ARG(QString, serial));
// }

void RobotServerPC::call_processSLAM()
{
    QMetaObject::invokeMethod(this, "processSLAM", Qt::QueuedConnection);
}

void RobotServerPC::call_slamPos(float x,
                                 float y,
                                 float z,
                                 float rx,
                                 float ry,
                                 float rz)
{
    QMetaObject::invokeMethod(
        this, "slamPos", Qt::QueuedConnection, Q_ARG(float, x), Q_ARG(float, y),
        Q_ARG(float, z), Q_ARG(float, rx), Q_ARG(float, ry), Q_ARG(float, rz));
}


void RobotServerPC::processPc_proc()
{
    while (true) {
        std::this_thread::sleep_for(std::chrono::milliseconds(821));
        if (!m_bRunRealsense) {
            continue;
        }

        processPc();
    }
}

void RobotServerPC::processPc()
{
    // sharememory of depth unlock
    m_smBaSend->lock();

    int nBaCloudSize = *(int*)(m_smBaSend->data());  // 开头4个字节是ba的大小

    // QByteArray baSend;
    // baSend.resize(nBaCloudSize);
    // memcpy(baSend.data(), (char*)(m_smBaSend->data()) + 4, nBaCloudSize);

    {
        std::unique_lock<std::mutex> lock(m_mutexBaCloud);
        m_baCloud.resize(nBaCloudSize);
        memcpy(m_baCloud.data(), (char*)(m_smBaSend->data()) + 4, nBaCloudSize);
    }

    // sharememory of depth unlock
    m_smBaSend->unlock();

    call_sendBaCloud();
}

void RobotServerPC::call_sendBaCloud()
{
    QMetaObject::invokeMethod(this, "sendBaCloud", Qt::QueuedConnection);
}

void RobotServerPC::sendBaCloud()
{
    std::unique_lock<std::mutex> lock(m_mutexBaCloud);
    static bool bx = false;
    if (!bx) {
        bx = true;
        qDebug() << "in processPc, baSend[0]:  " << quint8(m_baCloud.at(0));
        qDebug() << "in processPc, baSend[1]:  " << quint8(m_baCloud.at(1));
        qDebug() << "in processPc, baSend[2]:  " << quint8(m_baCloud.at(2));
        qDebug() << "in processPc, baSend[3]:  " << quint8(m_baCloud.at(3));
    }

    qint64 nWrite = 0;
    try {
        while (nullptr != m_socketCur && m_socketCur->isValid() &&
               nWrite < m_baCloud.size()) {
            nWrite += m_socketCur->write(m_baCloud.mid(nWrite));
            // qDebug() << __FUNCTION__ << "onCloudReady total: " <<
            // m_baCloud.size()
            //          << "  ,write: " << nWrite;
        }
    } catch (...) {
        qDebug() << "write cloud exception";
    }
}


#if 0
void RobotServerPC::setSendRsPosData(const QVariantMap& left,
                                     const QVariantMap& right,
                                     const QString& serial)
{
    s_bSendRsPos = true;
    s_strSerialSendRsPos = serial;
    s_fTargetXL = left.value("x").toFloat();
    s_fTargetYL = left.value("y").toFloat();
    s_fTargetZL = left.value("z").toFloat();
    s_fTargetXwL = left.value("l").toFloat();
    s_fTargetYwL = left.value("w").toFloat();
    s_fTargetZwL = left.value("h").toFloat();

    s_fTargetXR = right.value("x").toFloat();
    s_fTargetYR = right.value("y").toFloat();
    s_fTargetZR = right.value("z").toFloat();
    s_fTargetXwR = right.value("l").toFloat();
    s_fTargetYwR = right.value("w").toFloat();
    s_fTargetZwR = right.value("h").toFloat();

    qDebug() << "recv sendrspos "
             << "left: " << left;
    qDebug() << "recv sendrspos "
             << "right: " << right;
    qDebug() << "\n";
    m_bRunRealsense = true;
}

void RobotServerPC::trigger(int leftT, int rightT)
{
    if (s_listSteps.size() > 0) {
        QVariantMap mapCurStep = s_listSteps.at(s_nCurStepIndex).toMap();

        QVariantMap mapLeft = mapCurStep.value("left").toMap();
        QVariantMap mapRight = mapCurStep.value("right").toMap();

        quint8 nLeftToolControl =
            int(roundf(mapLeft.value("toolControl").toFloat()));
        quint8 nRightToolControl =
            int(roundf(mapRight.value("toolControl").toFloat()));

        QVariantMap mapLeftParm = mapCurStep.value("leftParm").toMap();
        QVariantMap mapRightParm = mapCurStep.value("rightParm").toMap();

        quint16 ftMode_L = int(roundf(mapLeftParm.value("ftMode").toFloat()));
        quint16 ftMode_R = int(roundf(mapRightParm.value("ftMode").toFloat()));

        quint16 periodMode_L =
            int(roundf(mapLeftParm.value("periodMode").toFloat()));
        quint16 periodMode_R =
            int(roundf(mapRightParm.value("periodMode").toFloat()));

        QVariantMap mapMode;
        mapMode.insert("ftMode_L", ftMode_L);
        mapMode.insert("ftMode_R", ftMode_R);
        mapMode.insert("periodMode_L", periodMode_L);
        mapMode.insert("periodMode_R", periodMode_R);
        m_robotCmdNode->sendCmd("part_trigger",
                                JsonUtil::map2jsonStr(mapMode).toStdString());

        if (leftT > 0 || rightT > 0) {
            m_robotCmdNode->clawControl(
                0, nLeftToolControl < 6 ? nLeftToolControl : 5);
            m_robotCmdNode->clawControl(
                1, nRightToolControl < 6 ? nRightToolControl : 5);

            if (nRightToolControl == 6)  // suojin 2
            {
                m_robotCmdNode->sendCmd("pinch", "180x2");
            } else if (nRightToolControl == 7)  // songkai
            {
                m_robotCmdNode->sendCmd("pinch", "songkai");
            } else if (nRightToolControl == 8)  // suojin 9
            {
                m_robotCmdNode->sendCmd("pinch", "180x9");
            }
        }
    }
}
#endif 

void RobotServerPC::processSLAM()
{
    m_smSLAM->lock();

    int nDataLen = *(int*)(m_smSLAM->data());  // 开头4个字节是ba的大小
    if (0 == nDataLen) {
        m_smSLAM->unlock();
        return;
    }

    QByteArray baSLAM;
    baSLAM.resize(nDataLen);
    memcpy(baSLAM.data(), (char*)(m_smSLAM->data()) + 4, nDataLen);

    m_smSLAM->unlock();

    qint64 nWrite = 0;
    try {
        while (nullptr != m_socketCur && m_socketCur->isValid() &&
               nWrite < baSLAM.size()) {
            nWrite += m_socketCur->write(baSLAM.mid(nWrite));
            // qDebug() << __FUNCTION__ << "onCloudReady total: " << baSLAM.size()
            //          << "  ,write: " << nWrite;
        }
    } catch (...) {
        qDebug() << "write SLAM Data exception";
    }
}

void RobotServerPC::slamPos(float x,
                            float y,
                            float z,
                            float rx,
                            float ry,
                            float rz)
{
    QVariantMap mapJson;
    mapJson.insert("action", "platformState");

    mapJson.insert("x", x);
    mapJson.insert("y", y);
    mapJson.insert("z", z);
    mapJson.insert("rx", rx);
    mapJson.insert("ry", ry);
    mapJson.insert("rz", rz);

    //    qDebug() << "platformState: " << mapJson;
    QByteArray ba = JsonUtil::packDataFE(mapJson, QByteArray());  // ef->fe

    qint64 nWrite = 0;
    while (nullptr != m_socketCur && m_socketCur->isValid() &&
           nWrite < ba.size()) {
        nWrite += m_socketCur->write(ba.mid(nWrite));
        // qDebug() << __FUNCTION__ << " total: " << ba.size()
        //          << "  ,write: " << nWrite;
    }
}
